#include <opencv2/opencv.hpp>
#include "zhnmat.hpp"
#include "utils.cpp"

int CHECKERBOARD[2]{6,9};
int main(int argc, char* argv[])
{
    using namespace std;
    using namespace cv;
    vector<vector<Point3f>> objpoints;
    vector<vector<Point2f>> imgpoints;
    vector<Point3f> objp;
    for(int i{0}; i<CHECKERBOARD[1]; i++)
        for(int j{0}; j<CHECKERBOARD[0]; j++)
            objp.push_back(Point3f(j,i,0));
    vector<String> images1, images2, images3;
    glob("/home/pi/Desktop/myfile/images/left/*.png", images1);
    glob("/home/pi/Desktop/myfile/images/right/*.png", images2);
    glob("/home/pi/Desktop/myfile/images/stero/*.png", images3);
    images1.insert(images1.end(), images3.begin(), images3.end());
    images2.insert(images2.end(), images3.begin(), images3.end());
    bool isCalibraLeft = (argc > 1 && stoi(argv[1])==1) ? true : false;
    PRINT_NAME_VALUE(isCalibraLeft);
    images3 = isCalibraLeft ? images1 : images2;
    for (int i=0; i<argc; i++)
        cout << argv[i] << endl;
    Mat origin, frame, gray;
    vector<Point2f> corner_pts;
    bool success;
    for(int i{0}; i<images3.size(); i++) {
        origin = imread(images3[i]);
        frame = isCalibraLeft ? origin(Rect(0,0,640,480)) : origin(Rect(640,0,640,480));
        cvtColor(frame,gray,COLOR_BGR2GRAY);
        success = findChessboardCorners(gray, Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
        if(success) {
            TermCriteria criteria(TermCriteria::EPS | TermCriteria::MAX_ITER, 30, 0.001);
            cornerSubPix(gray,corner_pts,Size(11,11), Size(-1,-1),criteria);
            drawChessboardCorners(frame, Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, success);
            objpoints.push_back(objp);
            imgpoints.push_back(corner_pts);
            if (argc > 2 && stoi(argv[2])==1){
                imshow("test", frame);
                waitKey(0);
            }
        }
        else
            cout << "detect chessboard failed.  " << images3[i] << endl;
    }
    Mat cameraMatrix,distCoeffs,R,T;
    Size imageSize = Size(gray.cols, gray.rows);
    double rms = calibrateCamera(objpoints, imgpoints, imageSize, cameraMatrix, distCoeffs, R, T, 0);
    Mat newCameraMatrix = getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1);
    zhnmat::Mat zhncameraMatrix = cvMat2zhnMat(cameraMatrix), zhnnewCameraMatrix = cvMat2zhnMat(newCameraMatrix);
    zhnmat::Mat::OutputFormat = zhnmat::OUTPUT_FORMAT::USE_BRACKET;
    PRINT_NAME_VALUE(rms);
    PRINT_NAME_VALUE(zhnnewCameraMatrix);
    PRINT_NAME_VALUE(cameraMatrix);
    PRINT_NAME_VALUE(zhncameraMatrix);
    PRINT_NAME_VALUE(distCoeffs);
    cout << "finished." << endl;
    Mat map1, map2;
    initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
                            getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
                            imageSize, CV_16SC2, map1, map2);
    for (int i=images3.size()-1; i>=0; --i){
        origin = imread(images3[i]);
        frame = isCalibraLeft ? origin(Rect(0,0,640,480)) : origin(Rect(640,0,640,480));
        remap(frame, origin, map1, map2, INTER_LINEAR);
    }
    return 0;
}
